#include "camera.hpp"
#include <iostream>
#include <pybind11/embed.h>
#include <pybind11/stl.h>
#include <filesystem>
#include <glog/logging.h>

camera::camera(int camera_id) : initialized(false)
{
    try
    {
        // 初始化Python解释器
        if (!Py_IsInitialized())
        {
            py::initialize_interpreter();
        }

        // 导入Python模块
        init_python();

        // 创建Camera实例 - 使用默认参数，只指定camera_id
        camera_instance = py_module.attr("Camera")(camera_id);

        // 启动摄像头捕获进程
        camera_instance.attr("start")();

        initialized = true;
    }
    catch (const std::exception &e)
    {
        LOG(ERROR) << "Error initializing camera: " << e.what();
        throw;
    }
}

camera::~camera()
{
    release();
}

void camera::init_python()
{
    try
    {
        // 获取Python sys模块来操作Python路径
        py::module sys = py::module::import("sys");

        // 获取包含cam.py的目录
        std::string module_dir = "/root/why_agv";

        // 将目录添加到Python路径
        sys.attr("path").attr("insert")(0, module_dir);

        // 导入cam模块
        py_module = py::module::import("cam");
    }
    catch (const std::exception &e)
    {
        LOG(ERROR) << "Failed to import Python module: " << e.what();
        throw;
    }
}

std::optional<std::pair<int, int>> camera::get_center(camera_color_en color)
{
    if (!initialized)
    {
        return std::nullopt;
    }

    try
    {
        py::object result = camera_instance.attr("get_center")((int)color);

        // 检查返回值是否为None
        if (result.is_none())
        {
            return std::nullopt;
        }

        // 将Python元组转换为C++对
        py::tuple center_tuple = result.cast<py::tuple>();
        int x = center_tuple[0].cast<int>();
        int y = center_tuple[1].cast<int>();

        return std::make_pair(x, y);
    }
    catch (const std::exception &e)
    {
        LOG(ERROR) << "Error getting center: " << e.what();
        return std::nullopt;
    }
}





int camera::get_color(int area)
{
    if (!initialized)
    {
        return -1;
    }

    try
    {
        py::object result = camera_instance.attr("get_color")(area);

        // 检查返回值是否为None
        if (result.is_none())
        {
            return -1;
        }

        // 将Python元组转换为C++对
        int res = result.cast<int>();
        return res;
    }
    catch (const std::exception &e)
    {
        LOG(ERROR) << "Error getting color: " << e.what();
        return -1;
    }
}

double camera::get_angle()
{
    if (!initialized)
    {
        return 0;
    }

    try
    {
        py::object result = camera_instance.attr("get_longest_line_angle")();

        // 检查返回值是否为None
        if (result.is_none())
        {
            return 0;
        }
        double res = result.cast<double>();
        return res;
    }
    catch (const std::exception &e)
    {
        LOG(ERROR) << "Error getting angle: " << e.what();
        return 0;
    }
}


std::optional<int> camera::take_pic(int name)
{
    if (!initialized)
    {
        return std::nullopt;
    }

    try
    {
        py::object result = camera_instance.attr("take_pic")(name);
    }
    catch (const std::exception &e)
    {
        LOG(ERROR) << "Error getting color: " << e.what();
        return std::nullopt;
    }
}


void camera::release()
{
    if (!initialized)
        return;

    try
    {
        // 停止摄像头捕获进程
        camera_instance.attr("stop")();
        // 释放摄像头资源
        camera_instance.attr("release")();

        camera_instance.release();
        initialized = false;
        
        if (Py_IsInitialized())
        {
            py::finalize_interpreter();
        }
    }
    catch (const std::exception &e)
    {
        LOG(ERROR) << "Error releasing camera: " << e.what();
    }
}


yj_return camera::is_move(int color)
{

    if (!initialized)
    {
        return YJ_INVALID;
    }

    try
    {
        py::object result = camera_instance.attr("if_move")(color);
        auto res = result.cast<std::string>();
        if(res == "True")
        {
            return YJ_OK;
        }
        else
        {
            return YJ_ERROR;
        }
    }
    catch (const std::exception &e)
    {
        LOG(ERROR) << "Error is_move: " << e.what();
        return YJ_INVALID;
    }
}
